package math;

public class mat3
{
    
    public static final int NUM_ROWS = 3;
    public static final int NUM_COLS = 3;
    
    public static final int NUM_ENTRIES = mat3.NUM_ROWS * mat3.NUM_COLS;
    
    public static final mat3 IDENTITY = new mat3();
    
    public static final mat3 ZERO = new mat3(0.0);
    
    
    /**
    Array containing matrix entries. Index-form:
        0   3   6
        1   4   7
        2   5   8
    */
    public double[] m;
    
    
    public mat3()
    {
        this(1.0);
    }
    
    public mat3(double aEntry)
    {
        this.m = new double[mat3.NUM_ENTRIES];
        this.m[0] = this.m[4] = this.m[8] = aEntry;
    }
    
    public mat3(mat2 aOther)
    {
        this(1.0);
        this.set(aOther);
    }
    
    
    public final void set(mat3 aOther)
    {
        System.arraycopy(aOther.m, 0, this.m, 0, mat3.NUM_ENTRIES);
    }
    
    public final void set(mat2 aOther)
    {
        // Copy the 2x2 matrix to the submatrix of this one:
        System.arraycopy(aOther.m, 0, this.m, 0, mat2.NUM_ROWS); // 1st column
        System.arraycopy(aOther.m, 2, this.m, 3, mat2.NUM_ROWS); // 2nd column
    }
    
    public final void setRow(int i, vec3 row)
    {
        this.m[i + 3 * 0] = row.x;
        this.m[i + 3 * 1] = row.y;
        this.m[i + 3 * 2] = row.z;
    }
    
    public final void setCol(int j, vec3 col)
    {
        this.m[j * 3 + 0] = col.x;
        this.m[j * 3 + 1] = col.y;
        this.m[j * 3 + 2] = col.z;
    }
    
    
    public void mul(mat3 aOther)
    {
        mat3 mul = mat3.mul(this, aOther);
        this.set(mul);
    }
    
    public void mulEntrywise(mat3 aOther)
    {
        for (int i = 0; i < mat3.NUM_ENTRIES; i++)
        {
            double ithMultiply = this.m[i] * aOther.m[i];
            this.m[i] = ithMultiply;
        }
    }
    
    public void transpose()
    {
        mat3.swap(this, 1, 3); // Swap 1 <-> 3.
        mat3.swap(this, 2, 6); // Swap 2 <-> 6.
        mat3.swap(this, 5, 7); // Swap 5 <-> 7.
    }
    
    
    public vec3 mul(vec3 aVec)
    {
        return new vec3(
            this.m[0] * aVec.x + this.m[3] * aVec.y + this.m[6] * aVec.z, 
            this.m[1] * aVec.x + this.m[4] * aVec.y + this.m[7] * aVec.z, 
            this.m[2] * aVec.x + this.m[5] * aVec.y + this.m[8] * aVec.z
        );
    }
    
    
    public mat3 copy()
    {
        mat3 copy = new mat3();
        copy.set(this);
        return copy;
    }
    
    
    public String toString()
    {
        StringBuilder sb = new StringBuilder();
        sb.append("mat3");
        for (int j = 0; j < mat3.NUM_ROWS; j++)
        {
            sb.append("\n   ");
            for (int i = 0; i < mat3.NUM_COLS; i++)
            {
                int ind = 3 * i + j;
                sb.append(String.format("%.2f", this.m[ind]));
                if (i < mat3.NUM_COLS - 1)
                {
                    sb.append(", ");
                }
            }
        }
        return sb.toString();
    }
    
    
    
    public static mat3 asRows(vec3 aA, vec3 aB, vec3 aC)
    {
        mat3 m = new mat3();
        m.setRow(0, aA);
        m.setRow(1, aB);
        m.setRow(2, aC);
        return m;
    }
    
    public static mat3 asCols(vec3 aA, vec3 aB, vec3 aC)
    {
        mat3 m = new mat3();
        m.setCol(0, aA);
        m.setCol(1, aB);
        m.setCol(2, aC);
        return m;
    }
    
    public static mat3 mul(mat3 aA, mat3 aB)
    {
        mat3 mul = new mat3();
        mat3.mul(aA, aB, mul);
        return mul;
    }
    
    public static mat3 rotate(vec3 aAxis, double aAngleRad)
    {
        double halfAngle = 0.5 * aAngleRad;
        double sinHalfAngle = math.sin(halfAngle);
        double p = aAxis.x * sinHalfAngle;
        double q = aAxis.y * sinHalfAngle;
        double r = aAxis.z * sinHalfAngle;
        double s = math.cos(halfAngle);
        mat3 rot = new mat3();
        rot.m[0] = 1 - 2 * q * q - 2 * r * r;
        rot.m[3] = 2 * p * q - 2 * s * r;
        rot.m[6] = 2 * p * r + 2 * s * q;
        rot.m[1] = 2 * p * q + 2 * s * r;
        rot.m[4] = 1 - 2 * p * p - 2 * r * r;
        rot.m[7] = 2 * q * r - 2 * s * p;
        rot.m[2] = 2 * p * r - 2 * s * q;
        rot.m[5] = 2 * q * r + 2 * s * p;
        rot.m[8] = 1 - 2 * p * p - 2 * q * q;
        return rot;
    }
    
    public static mat3 rotateX(double aAngleRad)
    {
        double halfAngle = 0.5 * aAngleRad;
        double sinHalfAngle = math.sin(halfAngle);
        double p = sinHalfAngle;
        double s = math.cos(halfAngle);
        mat3 rot = new mat3();
        rot.m[4] = 1 - 2 * p * p;
        rot.m[7] = -2 * s * p;
        rot.m[5] = 2 * s * p;
        rot.m[8] = 1 - 2 * p * p;
        return rot;
    }
    
    public static mat3 rotateY(double aAngleRad)
    {
        double halfAngle = 0.5 * aAngleRad;
        double sinHalfAngle = math.sin(halfAngle);
        double q = sinHalfAngle;
        double s = math.cos(halfAngle);
        mat3 rot = new mat3();
        rot.m[0] = 1 - 2 * q * q;
        rot.m[6] = 2 * s * q;
        rot.m[2] = -2 * s * q;
        rot.m[8] = 1 - 2 * q * q;
        return rot;
    }
    
    public static mat3 rotateZ(double aAngleRad)
    {
        double halfAngle = 0.5 * aAngleRad;
        double sinHalfAngle = math.sin(halfAngle);
        double r = sinHalfAngle;
        double s = math.cos(halfAngle);
        mat3 rot = new mat3();
        rot.m[0] = 1 - 2 * r * r;
        rot.m[3] = -2 * s * r;
        rot.m[1] = 2 * s * r;
        rot.m[4] = 1 - 2 * r * r;
        return rot;
    }
    
    public static mat3 rotateZYX(vec3 aAnglesRad)
    {
        mat3 rx = mat3.rotateX(aAnglesRad.x);
        mat3 ry = mat3.rotateY(aAnglesRad.y);
        mat3 rz = mat3.rotateZ(aAnglesRad.z);
        mat3 rzy = mat3.mul(rz, ry);
        mat3 rzyx = mat3.mul(rzy, rx);
        return rzyx;
    }
    
    public static mat3 rotateXYZ(vec3 aAnglesRad)
    {
        mat3 rx = mat3.rotateX(aAnglesRad.x);
        mat3 ry = mat3.rotateY(aAnglesRad.y);
        mat3 rz = mat3.rotateZ(aAnglesRad.z);
        mat3 rxy = mat3.mul(rx, ry);
        mat3 rxyz = mat3.mul(rxy, rz);
        return rxyz;
    }
    
    public static mat3 scale(vec3 aScale)
    {
        mat3 scale = new mat3();
        scale.m[0] = aScale.x;
        scale.m[4] = aScale.y;
        scale.m[8] = aScale.z;
        return scale;
    }
    
    public static mat3 transl(vec2 aTransl)
    {
        mat3 transl = new mat3();
        transl.m[6] = aTransl.x;
        transl.m[7] = aTransl.y;
        return transl;
    }
    
    
    
    private static void mul(mat3 aA, mat3 aB, mat3 aDST)
    {
        for (int i = 0; i < mat3.NUM_ENTRIES; i++)
        {
            int imod3 = i % 3;
            int idiv3 = i / 3;
            double dotProduct = 0.0;
            for (int j = 0; j < 3; j++)
            {
                int indA = j * 3 + imod3;
                int indB = j + 3 * idiv3;
                double jthMultiply = aA.m[indA] * aB.m[indB];
                dotProduct += jthMultiply;
            }
            aDST.m[i] = dotProduct;
        }
    }
    
    private static void swap(mat3 aA, int indA, int indB)
    {
        double t = aA.m[indA];
        aA.m[indA] = aA.m[indB];
        aA.m[indB] = t;
    }
    
}
